
#include <stdlib.h>
#include <functional>
#include "gpio.h"

#include "odrive_main.h"
#include "utils.hpp"
#include "communication/interface_can.hpp"

Axis::Axis(int axis_num,
           const AxisHardwareConfig_t& hw_config,
           Config_t& config,
           Encoder& encoder,
           Controller& controller,
           Motor& motor)
    : axis_num_(axis_num),
      hw_config_(hw_config),
      config_(config),
      encoder_(encoder),
      controller_(controller),
      motor_(motor)
{
    encoder_.axis_ = this;
    controller_.axis_ = this;
    motor_.axis_ = this;
    watchdog_feed();
}




static void step_cb_wrapper(void* ctx) {
    reinterpret_cast<Axis*>(ctx)->step_cb();
}


// @brief Does Nothing
void Axis::setup() {
    // Does nothing - Motor and encoder setup called separately.
}

static void run_state_machine_loop_wrapper(void* ctx) {
    reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
    reinterpret_cast<Axis*>(ctx)->thread_id_valid_ = false;
}

// @brief Starts run_state_machine_loop in a new thread
void Axis::start_thread() {
    osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t));
    thread_id_ = osThreadCreate(osThread(thread_def), this);
    thread_id_valid_ = true;
}

// @brief Unblocks the control loop thread.
// This is called from the current sense interrupt handler.
void Axis::signal_current_meas() {
    if (thread_id_valid_)
        osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS);
}

// @brief Blocks until a current measurement is completed
// @returns True on success, false otherwise
bool Axis::wait_for_current_meas() {
    return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal;
}




// @brief Do axis level checks and call subcomponent do_checks
// Returns true if everything is ok.
bool Axis::do_checks() {
    if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED))
        // motor got disarmed in something other than the idle loop
        error_ = ERROR_MOTOR_DISARMED;
   /*  if (!(vbus_voltage >= odrv.config_.dc_bus_undervoltage_trip_level))
        error_ = ERROR_DC_BUS_UNDER_VOLTAGE;
    if (!(vbus_voltage <= odrv.config_.dc_bus_overvoltage_trip_level))
        error_ = ERROR_DC_BUS_OVER_VOLTAGE; */
    /*if (!HAL_GPIO_ReadPin(ALL_STOP_GPIO_Port,ALL_STOP_Pin))
    {
        error_ |= ERROR_STOP;
    }*/
    
    // Sub-components should use set_error which will propegate to this error_
    motor_.do_checks();

    return check_for_errors();
}
/********************************************************************************************
* Function Name  : estimate_Convert_Speed
* Description    : ?��?�� ��a odrive?��
* Input		   : speed
* Output		   : rpm
* Return		   : 
*********************************************************************************************/
float Axis::Speed_Convert_estimate(int16_t val)  
{
    //float circumference;
			
	float c;

	//c = (float)(2.2905f * val * 0.001f); 
	c = (float)(1.872411127f * val * 0.001f); 
	//if((int)(c*10)%10 >= 5)
	//	circumference = (int)(c) + 1;
	//else
	//	circumference = (int)(c);
		  
	return c;
}   

bool Axis::check_axis_status(){
    
    if(axes[0]->current_state_ != axes[1]->current_state_){

		if(axes[0]->differ_state_ < 160)
			axes[0]->differ_state_++;
    }
    else{
		
		axes[0]->differ_state_ = 0;

	}
	if(axes[0]->differ_state_ >= 160)
	{
	    /* axes[0]->error_ |= ERROR_OVER_TEMP;
		axes[1]->error_ |= ERROR_OVER_TEMP; */
	}

	return true;
}
// @brief Update all esitmators
bool Axis::do_update_vel(){
    if(controller_.input_vel_can_ == 0U)//can�ڽ��յ�0�ٶ�
	{	

		controller_.can_idle_times++;
		
		if(controller_.can_idle_times == 16000U)	//2S
		{
			 requested_state_ = AXIS_STATE_IDLE;
		}
		else if(controller_.can_idle_times > 16000U)
		{
			controller_.can_idle_times = 16000U + 1U;
		}	

		controller_.input_vel_ = 0.0f;
	}
	else   //CAN�ڽ��յ����ٶȲ�Ϊ0
	{

		controller_.can_idle_times = 0U;
		
        controller_.can_com_times++;
		
		if(controller_.can_com_times < 6400U)  //˵��CAN�����յ����� 160 = 20ms 
		{
			if(current_state_ != AXIS_STATE_CLOSED_LOOP_CONTROL)
			{
				requested_state_ = AXIS_STATE_CLOSED_LOOP_CONTROL;
			}
			controller_.input_vel_ = Speed_Convert_estimate(controller_.input_vel_can_);  
		}
		else if(controller_.can_com_times == 6400U)   //˵�� CAN 6400Ϊ800ms û���յ�����
		{
			controller_.input_vel_ = 0.0f;
		}
		else if(controller_.can_com_times == 16000U)   //˵�� CAN 2S û���յ�����
		{
            requested_state_ = AXIS_STATE_IDLE;
			controller_.input_vel_ = 0.0f;
		}
		else if(controller_.can_com_times > 16000U)   //�������
		{
			controller_.can_com_times = 16000U + 1U;     
		}

	}

	return true;
}


bool Axis::do_updates() {
    // Sub-components should use set_error which will propegate to this error_
    /* for (ThermistorCurrentLimiter* thermistor : thermistors_) {
        thermistor->update();
    } */

    encoder_.update();
    
    
    //min_endstop_.update();
    //max_endstop_.update();

	if(config_.enable_debug)  //�����ٶȴ���
	{
	    do_update_vel();
	}
	
    if(config_.enable_Timestamp == 1){  //ֻ��һ����,�ϴ�����
		
	    odCAN->send_encoder_count(this); 	
        
	    //if(config_.enable_debug) 
	    //    check_axis_status();  //����������״̬
	        
	}		
   

	bool ret = check_for_errors();  //������

	if(ret == false)   //�д�
	
	    odCAN->send_error(this);
	
    else{              //�Ѿ������
    
        if(flag_axis_en)
		    odCAN->send_error(this);

	}
	return ret;
}

// @brief Feed the watchdog to prevent watchdog timeouts.
void Axis::watchdog_feed() {
    watchdog_current_value_ = get_watchdog_reset();
}

// @brief Check the watchdog timer for expiration. Also sets the watchdog error bit if expired.
bool Axis::watchdog_check() {
    if (!config_.enable_watchdog) return true;

    // explicit check here to ensure that we don't underflow back to UINT32_MAX
    if (watchdog_current_value_ > 0) {
        watchdog_current_value_--;
        return true;
    } else {
        error_ = ERROR_WATCHDOG_TIMER_EXPIRED;
        return false;
    }
}


bool Axis::run_closed_loop_control_loop() {

    if (!controller_.select_encoder(this->axis_num_)) {
        return error_ = ERROR_CONTROLLER_FAILED, false;
    }

    // To avoid any transient on startup, we intialize the setpoint to be the current position
    if (controller_.config_.circular_setpoints) {
        if (!controller_.pos_estimate_circular_src_) {
            return error_ = ERROR_CONTROLLER_FAILED, false;
        }
        else {
            controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
            controller_.input_pos_ = *controller_.pos_estimate_circular_src_;
        }
    }
    else {
        if (!controller_.pos_estimate_linear_src_) {
            return error_ = ERROR_CONTROLLER_FAILED, false;
        }
        else {
            controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
            controller_.input_pos_ = *controller_.pos_estimate_linear_src_;
        }
    }
    controller_.input_pos_updated();

    // Avoid integrator windup issues
    controller_.vel_integrator_torque_ = 0.0f;


    run_control_loop([this](){
        // Note that all estimators are updated in the loop prefix in run_control_loop
			
        if (!controller_.update(&torque_setpoint))
            return error_ = ERROR_CONTROLLER_FAILED, false;
            
		
        float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
		
        if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
            return false; // set_error should update axis.error_

        return true;
    });
    return check_for_errors(); 
}


// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
// When pressed, set the linear count to the offset (default 0), and then go to position 0

bool Axis::run_idle_loop() {
    // run_control_loop ignores missed modulation timing updates
    // if and only if we're in AXIS_STATE_IDLE
    safety_critical_disarm_motor_pwm(motor_);
	
    run_control_loop([this]() {
        //encoder_.abi_calibration();             //绝对式编码器 校准
        if((num>=0) && (num<=5000)){
            DC_sum_phB += motor_.current_meas_.phB;
            DC_sum_phC += motor_.current_meas_.phC;
            num++;
            motor_.DC_calib_.phB = DC_sum_phB/(float)num;                   //多次采样采样电阻两端的静态偏置电压，求取平均值
            motor_.DC_calib_.phC = DC_sum_phC/(float)num;
        }
        if(num>=5000 && num<=6000)
        {
            num++;
            if(fabs(motor_.current_meas_.phB - motor_.DC_calib_.phB)>=0.5f)
            {
                num=-1;
                error_ = ERROR_CURRENT_MEASURE_FAULT;
            }
            if(fabs(motor_.current_meas_.phC - motor_.DC_calib_.phC)>=0.5f)
            {
                num=-1;
                error_ = ERROR_CURRENT_MEASURE_FAULT;
            }
        }
        if(num>=6001){
            num=-1;
            motor_.current_control_.is_ready = true;        //上电 电流采样偏执校准 
        }
        if(motor_.current_control_.is_ready == true)
            return false;
        return true;
    });
    return check_for_errors();
    return true;
}
bool Axis::run_zero_test(){
    const float start_lock_duration = 4.0f;
    float voltage_magnitude = motor_.config_.calibration_current * motor_.config_.phase_resistance*2;
    // go to motor zero phase for start_lock_duration to get ready to scan
    int i = 0;
    run_control_loop([&](){
        if (!motor_.enqueue_voltage_timings(voltage_magnitude, 0.0f))
            return false; // error set inside enqueue_voltage_timings
        motor_.log_timing(TIMING_LOG_ENC_CALIB);
        return ++i < start_lock_duration * current_meas_hz;
    });
    if (error_ != Axis::ERROR_NONE)
        return false;
}

// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
  // arm!
    motor_.arm();

    /*for(;;)
    {
		run_idle_loop();
		
		motor_.arm(); 
		
	}*/
    for (;;) {
        if (requested_state_ != AXIS_STATE_UNDEFINED) {
            size_t pos = 0;
            if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
                task_chain_[pos++] = AXIS_STATE_IDLE;
                task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
                task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
                task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
            } else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
                task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
                task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
                task_chain_[pos++] = AXIS_STATE_IDLE;
            } else if (requested_state_ != AXIS_STATE_UNDEFINED) {
                task_chain_[pos++] = requested_state_;
                task_chain_[pos++] = AXIS_STATE_IDLE;
            }
            task_chain_[pos++] = AXIS_STATE_UNDEFINED;  // TODO: bounds checking
            requested_state_ = AXIS_STATE_UNDEFINED;
            //error_ &= ~ERROR_INVALID_STATE;
        }

        bool status;
        switch (current_state_) {
            case AXIS_STATE_MOTOR_CALIBRATION: {
                if ( !motor_.current_control_.is_ready)
                    goto invalid_state_label;
                status = motor_.run_calibration();
                printf("run_calibration:%d\r\n",status);
            } break;

            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
                if ( !motor_.is_calibrated_ || !motor_.current_control_.is_ready)
                    goto invalid_state_label;
                status = encoder_.run_offset_calibration();
                printf("run_offset_calibration:%d\r\n",status);
            } break;

            case AXIS_STATE_CLOSED_LOOP_CONTROL: {
                if (!motor_.is_calibrated_ || motor_.config_.direction==0 || !motor_.current_control_.is_ready)
                    goto invalid_state_label;
                if (!encoder_.is_ready_)
                    goto invalid_state_label;
                watchdog_feed();
                status = run_closed_loop_control_loop();
            } break;

            case AXIS_STATE_IDLE: {
                run_idle_loop();
                status = motor_.arm(); // done with idling - try to arm the motor
            } break;
            case AXIS_STATE_ZERO_TEST:{
                if ( !motor_.is_calibrated_ )
                    goto invalid_state_label;
                status = run_zero_test();
            }break;

            default:
            invalid_state_label:
                error_ = ERROR_NONE;
                status = false;  // this will set the state to idle
                break;
        }
        // If the state failed, go to idle, else advance task chain
        if (!status) {
            std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
            current_state_ = AXIS_STATE_IDLE;
        } else {
            std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
            task_chain_.back() = AXIS_STATE_UNDEFINED;
        }
    }
}


